#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class DemoNode : public rclcpp::Node
{
public:
    DemoNode() : Node("demo_node_cpp"), count_(0)
    {
        RCLCPP_INFO(this->get_logger(), "DemoNode C++ 已启动！");

        // 发布器
        publisher_ = this->create_publisher<std_msgs::msg::String>("demo_topic_cpp", 10);

        // 订阅器
        subscription_ = this->create_subscription<std_msgs::msg::String>(
            "demo_topic_cpp",
            10,
            [this](const std_msgs::msg::String::SharedPtr msg) {
                RCLCPP_INFO(this->get_logger(), "收到消息: '%s'", msg->data.c_str());
            });

        // 定时器，每秒调用一次
        timer_ = this->create_wall_timer(
            1s,
            [this]() { this->timer_callback(); });
    }

private:
    void timer_callback()
    {
        auto message = std_msgs::msg::String();
        message.data = "Hello ROS 2 C++! count: " + std::to_string(count_++);
        publisher_->publish(message);
        RCLCPP_INFO(this->get_logger(), "发布消息: '%s'", message.data.c_str());
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    size_t count_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DemoNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
